/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.techhighteam675.robot2012;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 * @author Noah Picard
 */
class BeltSystem {
    public static BeltSystem INSTANCE = new BeltSystem();

    Victor pickupMotor, shootingMotor;
    boolean beltFirstRun;
    boolean beltDump;

    // Scoop belt
    boolean pickupControl;
    Timer pickupTime;

    // Shoot belt
    Timer shootTime;
    
    public BeltSystem() {
        pickupMotor = new Victor(Definitions.PAYLOAD_DIGITAL_MODULE, Definitions.BELT_MOTOR_PICKUP);
        shootingMotor = new Victor(Definitions.PAYLOAD_DIGITAL_MODULE, Definitions.BELT_MOTOR_SHOOTING);

        beltFirstRun = false;
        beltDump = false;

        pickupControl = false;
        pickupTime = new Timer();
        shootTime = new Timer();
        pickupTime.reset();
        shootTime.reset();
    }
    // Update Belts
    public void update() {
        int pickupSetting = 0;
        int shootSetting = 0;

        if(pickupControl) {
            pickupSetting = Definitions.BELT_PICKUP_FORWARD;
        }
        if(shootTime.get() < Definitions.BELT_SHOOT_TIME && beltFirstRun)
        {
            if(pickupTime.get() < Definitions.BELT_PICKUP_TIME) {
                pickupSetting = Definitions.BELT_PICKUP_REVERSE;
            }
            else {
                pickupSetting = 0;
            }

            shootSetting = Definitions.BELT_SHOOT_FORWARD;
        }

        if(beltDump)
        {
            pickupSetting = Definitions.BELT_PICKUP_REVERSE;
            shootSetting = Definitions.BELT_SHOOT_REVERSE;
        }

        pickupMotor.set(pickupSetting * Definitions.BELT_PICKUP_SPEED);
        shootingMotor.set(shootSetting * Definitions.BELT_SHOOT_SPEED);

        pickupControl = false;
        beltDump = false;

        if(Definitions.DEBUG_MODE)
        {
            SmartDashboard.putString("Pickup Belt", String.valueOf(pickupSetting));
            SmartDashboard.putString("Shooting Belt", String.valueOf(shootSetting));
        }
        else
        {
            switch(pickupSetting)
            {
            case Definitions.BELT_PICKUP_FORWARD:
                SmartDashboard.putString("Pickup Belt", "Sucking");
            break;

            case Definitions.BELT_PICKUP_REVERSE:
                SmartDashboard.putString("Pickup Belt", "Spitting");
            break;

            default:
                SmartDashboard.putString("Pickup Belt", "Off");
            break;
            }

            if(shootSetting == Definitions.BELT_SHOOT_FORWARD) {
                SmartDashboard.putString("Shooting Belt", "Shooting");
            }
            else {
                SmartDashboard.putString("Shooting Belt", "Off");
            }
        }
    }
    // Run Pickup Belt
    public void pickup() {
        pickupControl = true;
    }
    // Run Shooting Belt
    public void shoot() {
        if(shootTime.get() > Definitions.BELT_SHOOT_TIME || !beltFirstRun) {
            beltFirstRun = true;

            pickupTime.reset();
            pickupTime.start();
            shootTime.reset();
            shootTime.start();
        }
    }
    // Emergency dump
    public void dump() {
        beltDump = true;
    }
}
